/*
    Copyright 2006-2011 Patrik Jonsson, sunrise@familjenjonsson.org

    This file is part of Sunrise.

    Sunrise is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 3 of the License, or
    (at your option) any later version.

    Sunrise is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Sunrise.  If not, see <http://www.gnu.org/licenses/>.

*/

/// \file
/// Implementations of some handy vector operations.

// $Id$

#include "vecops.h"

#include "blitz/array.h"
#include "interpolatort.h"
#include "mcrx-debug.h"
#include "boost/foreach.hpp"
#include "boost/function.hpp"

using namespace blitz; 

mcrx::T_float mcrx::mag(const vec3d& v)
{
  return sqrt(dot(v,v));
}


mcrx::vec3d mcrx::rotate(const mcrx::vec3d& v, 
			 const mcrx::vec3d& newz, const mcrx::vec3d& newx,
			 bool inverse)
{
  // check for obvious input parameter screwups
  assert(abs(dot(newz, newz)- 1 < 1e-5));
  assert(abs(dot(newx, newx)- 1 < 1e-5));
  assert(abs(dot(newz, newx) < 1e-5));

  // The third axis ("newy") in the new system is the cross product of
  // the two first
  const vec3d newy(cross(newz, newx));
  
  // For the forward transformation, perform the matrix multiplication
  // in written-out form. For the inverse transformation, calculate
  // the components by dotting the vector with the axes.
  const vec3d newdir= inverse ? 
    vec3d(dot(v,newx), dot(v,newy), dot(v,newz)) :
    vec3d(v[0]*newx + v[1]*newy + v[2]*newz);
  
  return newdir;
}

mcrx::vec3d mcrx::rotate(const mcrx::vec3d& v, const mcrx::vec3d& newz)
{
  assert(abs(dot(newz, newz)- 1 < 1e-5));

  // Calculate the orthogonal projection of the x-axis in the new system.
  const vec3d x(1.,0.,0.);
  const vec3d k1(x- newz*dot(x, newz) );

  // If newz is nearly aligned with the x-axis this will be a singular
  // system, so we need to check for this.
  const T_float mag_k1 = sqrt (dot (k1, k1)); 
  if(mag_k1 > 1e-5)
    // no singularity problem, so call the long version of rotate
    return rotate(v, newz, k1/mag_k1);
  else {
    // singular -- redo with the second axis as -z
    const vec3d mz(0.,0.,-1.);
    const vec3d k2(mz- newz*dot(mz, newz) );
    const T_float mag_k2 = sqrt (dot (k2, k2)); 
    assert(mag_k2 > 0.9);

    // now rotate 
    return rotate(v, newz, k2/mag_k2);
  }
}


mcrx::array_1 mcrx::interpolate_onto(const array_1& x, const array_1& y,
				     const array_1& newx,
				     bool logarithmic)
{
  interpolator<T_float, T_float, 1> interpol;
  array_1 newy(newx.shape());
  if (logarithmic) {
    ASSERT_ALL(x>0);
    ASSERT_ALL(y>=0);
    ASSERT_ALL(newx>0);
    //can't use range constructor because array iterator is buggy
    vector<T_float> xax;
    for(int i=0;i<x.size();i++)
      xax.push_back(log(x(i)));
    interpol.setAxes(xax);
    interpol.setPoints(array_1(where(y>0,log(y),-blitz::huge(T_float()))));
    newy = exp(interpol.interpolate(log(newx)));
  }
  else {
    vector<T_float> xax;
    for(int i=0;i<x.size();i++)
      xax.push_back(x(i));
    interpol.setAxes(xax);
    interpol.setPoints(y);
    newy = interpol.interpolate(newx);
  }
  ASSERT_ALL(newy==newy);
  return newy;
}


std::vector<mcrx::T_float> 
mcrx::calculate_middle(const std::vector<mcrx::T_float>& v) 
{
  vector<mcrx::T_float> m(v.size());
  if(v.size()==1) {
    // for 1-element vectors, the middle is the time itself...
    m[0]=v[0];
  }
  else {
    m [0] =  blitz::neghuge(mcrx::T_float()); // first starts at -inf
    for (int i = 1; i < v.size(); ++i){
      m [i] = (v [i- 1] + v [i])/2;
    }
  }
  // final time lasts until Infinity but is not in the vector... 
  return m;
}


/** Integrates a function defined by the x, y arrays between xmin and
    xmax. If logarithmic is true, the function is assumed to be
    piecewise power-law, otherwise piecewise linear.  The segments
    containing xmin and xmax must be supplied (idx=1 means the point
    is between (inclusive) x[1] and up to but not including x[2]), but
    if they are set to -1 the array will be searched (this involves a
    copy of the entire array due to the dumb blitz iterators, so if
    you are going to call it repeatedly it pays to manage this outside
    this function). Ported from python. */
mcrx::T_float mcrx::integrate_between(const array_1& y,
				      const array_1& x,
				      T_float xmin, T_float xmax,
				      const bool logarithmic,
				      int left_idx,
				      int right_idx)
{
  if(xmin==xmax)
    return 0;
  assert (xmin<xmax);
  assert (xmin>=x(0));
  assert (xmax<=x(x.size()-1));

  if ((left_idx<0) || (right_idx<0)) {
    // indices not given, find them

    // can't use range constructor because array iterator is buggy
    vector<T_float> tempx;
    tempx.reserve(x.size());
    for(int i=0;i<x.size();i++)
      tempx.push_back(log(x(i)));

    if (left_idx<0)
      left_idx = lower_bound(tempx.begin(), tempx.end(), xmin) - tempx.begin() -1;
    if (right_idx<0)
      right_idx = lower_bound(tempx.begin(), tempx.end(), xmax) - tempx.begin() -1;
  }

  // check indices
  assert(xmin>=x(left_idx));
  assert(xmin<x(left_idx+1));
  assert(xmax>=x(right_idx));
  assert(left_idx<x.size());

  T_float integral=0.0;

  boost::function<T_float(T_float,T_float,T_float, T_float)> f;
  if(logarithmic)
    f= static_cast<T_float(*)(T_float,T_float,T_float, T_float)>(&integrate_segment_log);
  else
    f= static_cast<T_float(*)(T_float,T_float,T_float, T_float)>(&integrate_segment_lin);

  // we just have to handle the partial segments on the edges

  // if left_idx==right_idx then the interval is within one segment
  if (left_idx==right_idx) {
    integral = f(xmin,
		 interpolate_point(x(left_idx), y(left_idx), 
				   x(left_idx+1),y(left_idx+1),
				   xmin,logarithmic),
		 xmax,
		 interpolate_point(x(left_idx), y(left_idx), 
				   x(left_idx+1),y(left_idx+1),
				   xmax,logarithmic));
    return integral;
  }
    
  // so interval is not confined within one segment
  // first deal with partial end segments
  integral += f(xmin,
		interpolate_point(x(left_idx), y(left_idx), 
				  x(left_idx+1), y(left_idx+1),
				  xmin,logarithmic),
		x(left_idx+1), y(left_idx+1));
  // there is a subtle issue here. If xmax is the end of the x-vector,
  // then we will overrun the array if we use right_idx+1, so we need
  // to check for that case
  if(x(right_idx)<xmax) {
    assert(xmax<x(right_idx+1));
    integral += f(x(right_idx), y(right_idx),
		  xmax,
		  interpolate_point(x(right_idx), y(right_idx),
				    x(right_idx+1), y(right_idx+1),
				    xmax,logarithmic));
  }

  // and then the interior segments, if any
  if (right_idx-left_idx>1) 
    integral += integrate_quantity(y(Range(left_idx+1, right_idx)),
				   x(Range(left_idx+1, right_idx)),
				   logarithmic);
  return integral;
}


mcrx::array_1 mcrx::resample(const array_1& data, const array_1& old_x,
			     const array_1& new_x, bool allow_outside,
			     bool logarithmic)
{
  assert(!allow_outside);
  return interpolate_onto(old_x,data,new_x,logarithmic);
}


mcrx::array_1 mcrx::subsample(const array_1& old_y, const array_1& old_x,
			      const array_1& new_x)
{
  assert(new_x(0)>=old_x(0));
  assert(new_x(new_x.size()-1)<=old_x(old_x.size()-1));

  const size_t n(new_x.size());
  array_1 new_y(n);

  // find starting indices in old_x -- we do linear search here because
  // we assume that the two vectors start approximately at the same
  // value
  T_float x1=new_x(0),x2;
  size_t old_idx1=0, old_idx2, new_idx=0;
  while (old_x(++old_idx1) <= x1); --old_idx1;
  old_idx2=old_idx1;
  // first bin is special because it's asymmetric and only half the width
  x2=0.5*(new_x(new_idx)+new_x(new_idx+1));
  while (old_x(++old_idx2) <= x2); --old_idx2;
  new_y(new_idx++) = integrate_between(old_y, old_x, 
				       x1, x2,
				       false, old_idx1, old_idx2)/
    (x2-x1);

  // now loop over bins
  while(new_idx<n-1) {
    // find next bin
    x1=x2;
    old_idx1=old_idx2;
    x2=0.5*(new_x(new_idx)+new_x(new_idx+1));
    while (old_x(++old_idx2) <= x2); --old_idx2;

    new_y(new_idx++) = integrate_between(old_y, old_x,
					 x1, x2,
					 false, old_idx1, old_idx2)/
      (x2-x1);
  }
   
  // and again last bin is special
  assert(new_idx==n-1);
  x1=x2;
  old_idx1=old_idx2;
  x2=new_x(n-1);
  while ((++old_idx2 < old_x.size()) && 
	 (old_x(old_idx2) <= x2)); --old_idx2;
  new_y(new_idx++) = integrate_between(old_y, old_x, 
				       x1, x2,
				       false, old_idx1, old_idx2)/
    (x2-x1);

  return new_y;
}


mcrx::array_1 mcrx::logspace(T_float xmin, T_float xmax, int N)
{
  array_1 x(N);

  const T_float dr= (log10(xmax)-log10(xmin))/(N-1);

  x = pow(10, tensor::i*dr+log10(xmin));

  // make sure the end points are *identical* regardless of truncation
  x(0) = xmin;
  x(N-1) = xmax;

  return x;
}


